#include "CSerialPort.h"
#include "plane.h"
#include "global.h"

using namespace std;
using namespace openni;
typedef pcl::PointXYZ PointT;

extern CSerialPort myserialport;


FindPlane::FindPlane() : viewer("plane Viewer") {}  // Construct a cloud viewer, with a window name

													   // ����ص�����cloud_cb_,��ȡ������ʱ�����ݽ��д���
void FindPlane::cloud_plane_(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr &cloud)
{
	if (!viewer.wasStopped())
	{
			pcl::PassThrough<PointT> pass_x,pass_y;                         //直通滤波对象
			pcl::VoxelGrid<PointT> sor;
			pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
			pcl::PointIndices::Ptr inliers(new pcl::PointIndices);
			pcl::SACSegmentationFromNormals<PointT, pcl::Normal> seg;
			pcl::PointCloud<PointT>::Ptr cloud_filtered(new pcl::PointCloud<PointT>);
			pcl::ExtractIndices<PointT> extract;
			pcl::PointIndices::Ptr inliers_plane(new pcl::PointIndices);
			pcl::NormalEstimation<PointT, pcl::Normal> ne;
			pcl::PointCloud<pcl::Normal>::Ptr cloud_normals(new pcl::PointCloud<pcl::Normal>);
			pcl::ModelCoefficients::Ptr coefficients_plane(new pcl::ModelCoefficients), coefficients_cylinder(new pcl::ModelCoefficients);
			pcl::search::KdTree<PointT>::Ptr tree(new pcl::search::KdTree<PointT>());

			char send_data[8];
			short x, z=0.5;
			pass_x.setInputCloud(cloud);
			pass_x.setFilterFieldName("y");
			pass_x.setFilterLimits(-0.08, 0.15);
			pass_x.filter(*cloud_filtered);
			
			sor.setInputCloud(cloud_filtered);
			sor.setLeafSize(0.012f, 0.012f, 0.012f);
			sor.filter(*cloud_filtered);
		

			ne.setSearchMethod(tree);
			ne.setInputCloud(cloud_filtered);
			ne.setKSearch(20);
			ne.compute(*cloud_normals);
			/*tt.tic();

			pcl::IntegralImageNormalEstimation<pcl::PointXYZ, pcl::Normal> ne;
			ne.setNormalEstimationMethod(ne.AVERAGE_3D_GRADIENT);
			ne.setMaxDepthChangeFactor(0.02f);
			ne.setNormalSmoothingSize(10.0f);
			ne.setInputCloud(cloud);
			ne.compute(*cloud_normals);
			std::cerr << "  2  Done: " << tt.toc() << " ms\n\n";*/

			seg.setOptimizeCoefficients(true);
			seg.setModelType(pcl::SACMODEL_NORMAL_PLANE);
			seg.setNormalDistanceWeight(0.1);
			seg.setMethodType(pcl::SAC_RANSAC);
			seg.setMaxIterations(100);
			seg.setDistanceThreshold(0.03);
			seg.setInputCloud(cloud_filtered);
			seg.setInputNormals(cloud_normals);
			// Obtain the plane inliers and coefficients
			seg.segment(*inliers_plane, *coefficients_plane);
			
			float a, b, c, d;
			short angle, distance;
			if (coefficients_plane->values.size() > 0)
			{
				a = coefficients_plane->values[0];
				b = coefficients_plane->values[1];
				c = coefficients_plane->values[2];
				d = coefficients_plane->values[3];
				angle = atan2(c, b) * 180 / 3.14 - 90;
				if(angle<-45)angle+=180;
				distance = (abs(d) / sqrt(a*a + b*b + c*c))*100;
				if(c!=0)cout << "角度: " << angle << endl;
				cout << "距离" << distance;
				angle=-angle;;
				send_data[0]=1;
				send_data[1] = angle;
				send_data[2] = (angle >> 8);
				send_data[3] = distance;
				send_data[4] = (distance >> 8);
				send_data[5] = 0;
				send_data[6] = 0;
			}
			else{
				send_data[0]=angle=distance=0;
			}
			send_data[1] = angle;
			send_data[2] = (angle >> 8);
			send_data[3] = distance;
			send_data[4] = (distance >> 8);
			send_data[5] = 0;
			send_data[6] = 0;
			send_data[7]=0x5A;

			myserialport.writeToCom_char((char*)&send_data,sizeof(send_data));

			//Extract the planar inliers from the input cloud
			extract.setInputCloud(cloud_filtered);
			extract.setIndices(inliers_plane);
			extract.setNegative(false);

			// Write the planar inliers to disk
			pcl::PointCloud<PointT>::Ptr cloud_plane(new pcl::PointCloud<PointT>());
			extract.filter(*cloud_plane);
			std::cerr << "    Done: " << tt.toc() << " ms\n\n";

			if(cloud_plane->size()>0)viewer.showCloud(cloud_plane);
			tt.tic();
	}
}

void FindPlane::plane_run()
{
	OpenNI::initialize();
	Array<DeviceInfo> aDeviceList;

	while(true)
	{
		OpenNI::enumerateDevices(&aDeviceList);
		while(aDeviceList.getSize()==0)
		{
			OpenNI::enumerateDevices(&aDeviceList);usleep(1);
		}
		// create a new grabber for OpenNI devices
		pcl::Grabber* interface = new pcl::io::OpenNI2Grabber();

		// make callback function from member function
	// make callback function from member function
		boost::function<void(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr&)> f =
		boost::bind(&FindPlane::cloud_plane_, this, _1);

		// connect callback function for desired signal
		boost::signals2::connection c = interface->registerCallback(f);

		// start receiving point clouds
		interface->start();

		while (!viewer.wasStopped())
		{
			OpenNI::enumerateDevices(&aDeviceList);
			if(aDeviceList.getSize()==0||mode != 0x03)break;
			boost::this_thread::sleep(boost::posix_time::seconds(0.001));
		}
		/*
		while (!viewer.wasStopped()&&flag!=1)
		{
		boost::this_thread::sleep(boost::posix_time::seconds(1));
		}
		*/
		// Stop the data acquisition
		interface->stop();
		delete interface;
				cout<<"plane break"<<endl;

		if(mode!=0x03)break;
	}
}

